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ABSTRACT 


A discrete state space model ts developed which describes an autonomous under- 
water vehicle and incorporates the effects of currents, sea state, and wind as it travels 
through the sea. Heading commands are calculated to overcome these effects by the 
design of an Extended Kalman Filter which estimates their combined velocity compo- 
nents. Simulations are done which test the filter’s effectiveness in a range of different 


environments. Some potential uses for this system are discussed at the end. 
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I. INTRODUCTION 


A. BACKGROUND 

With the increased costs for Manned Naval Suface and Subsurface assets and the 
greatly increased capability of small computing devices, using small autonomous under- 
water vehicles (AUVs) to carrvout certain missions has become increasingly desirable. 
A vehicle's capability to navigate to a precise location independently and in manv cases 
covertly 1s essential. 

Current research into potential navigation systems that measure a vehicle's position 
and unknown forces acting on it include sonar doppler, stapdown Inertial Measuring 
Units (IMUs), and sonar triangularization. The two primary IMUs underdevelopment 
fer VS are the Laser Ring Gyro and the Fiber Optic Gyro (FOG). The svstems 
mentioned above each have their own independent drift characteristics which have to 
be filtered out and their actual positions updated from time to time using an outside 
source such as the Navstar Global Positioning Svstem (GPS) or Omega. In addition, 
they have large power requirements, are bulky (with the possible exception of the FOG), 
and require compex control circuitry. Ifa microprocessor could be programmed to es- 
timate the underwater forces acting on the vehicle enroute to the destination, based on 
updates by GPS, and then to applv a heading command to compensate for any devi- 
ations in its course, then the power requirements, complexity, and cost could be 
signifcantly reduced. Figure | on page 2 illustrates a block diagram of a possible svs- 
tem. 

In this Thesis, a state space model will be developed based on a generic vehicle's 
dvnamics. The forces acting on the vehicle will be modeled as a velocity vector with an 


angle (Drift) and a magnitude (Set) with the following assumptions: 


e the angle and magnitude of this veetor are white random Gaussian variables with 
zero mean about predicted values, and 


e the overall Set and Drift between sample intervals remains a constant. 


In order to estimate this set and drift, an extended Kalman Filter is developed. 
Various simulations are performed which explore the behavior of the filter and its abilitv 
to estimate the velocity components. The simulation was done using DSL (Dynamic 
Simulation Language) on the Naval Postgraduate School’s IBM 3033 Mainframe Com- 


puter. 
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Bes 






HEADING COMMAND 


DES FINA 


Figure 1. System Block Diagram 


B. GLOBAL POSITIONING SYSTEM 

The Navstar Global Positioning System is a space-based radio navigation system 
which operates passively. GPS 1s a tri-service multiagency program inanaged by the Air 
monee s Fs Jomt Program Oihce. When fully operational in the early 1990s, the svs- 
tem will consist of 18 satellites with three orbiting spares, in 6 orbital planes. [Each sat- 
ellite transmits two unique codes, the clear/acquision (C/A) code and the Precise (P) 
code. These codes, which are unique to each satellite, are modulated and two L Band 
frequencies, L] and L2, as pseudo random noise sources along with the Navigation 
Message. L] contains both the C/A and P codes while L2 has just the P code. 

iiiemmscrs Tecelver Generates the identical codes and correlates its code with the 
satellite's. The amount of slewing theat the recciver must do in order to obtain a match 
is the time that the two signals are off. Dividing by the speed of hght and correcting for 
atmospheric delays and any clock differences produces the range to the satellite. Ob- 
taining three ranges from three different satellites is required to obtain a 3 dimensional 
coordinate position fix. However, in order to accomplish this, the receiver's clock would 
have to be identical in time to that of the GPS’s Master Clock which would defeat the 
purpose of the system. To provide this time differential, the receiver must lock onto 4 
satellites and solve four simultancous equations for the three coordinates and the tine 
guerential in order to get the fix. 

The GPS Navigation Message supplies the user with the satellite’s clock offset, at- 
mospheric delays, and satellite ephemeris in order to accurately solve the four equations. 
[tis modulated on both LI and L2 at a 50 bit per second rate and is contained in a 1500 
bit frame. This frame is divided into 5 subframes which contain specific blocks of data. 
This format is shown in Figure 2 on page 4. 

The first subframe contains the clock correction parameters and ionospheric propa- 
gation delay model parameters. The second and third subframes contain the satellite's 
ephemeris which is updated every 1-5 hours by the ground control segment of GPS. 
Subframe four is for any special messages which must be passed onto the users. 

The final subframe contains the almanac data. The almanac data is an abbreviated 
form of the first three subframes for each of the satellites. This data is transmitted on 
a rotating basis and requires 19 subframes to transmit it all and is used only to help lo- 
cate the four satellites which are overhead. The almanac must be loaded into the re- 
ceiver either manually or by the receiver itself by locking onto a satellite and obtaining 


the block five message through an entire cycle. 
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WeGhs RECEIVER 
PieeCndererO Pemlomm tne tasks that will be referred to later in this thesis, the GPS 
receiver must be capable of accomplishing the following tasks: 
¢ Obtain current position 
e Store the necessarv waypoints, including the start and end points. 
e Provide course information to next waypoint. 
¢ Have the ability to survive temperature, pressure, and other conditions associated 
with the marine environment. 

Currently under development for the Marine Corps bv the Naval Ocean Systems 
mermaid { VOSC) San Diego is a receiver called the Small Unit Navigation System 
(SUNS). [Ref.1] This receiver is designed to be handheld, rugged, lightweight, and bat- 
tery operated for the Marine Corp’s Reconnaissance teams during long range operations 
on both land and underwater. SUNS will be able to store up to waypoints. including 
the destination, and will provide the user with current position, velocity, cross track er- 
ror, distance and bearing to next waypoint, and time with a positional accuracy of 50 {t. 

One additional requirement which should be considered is equipping the receiver 
with a multi-channel capability to lock on all four satellites at the same time. This would 
allow the vehicle to obtain a position fix quicker and thus spend less time on or near the 
surface. The SUNS receiver is being designed as a single channel unit and will have to 


sequentially correlate each of the signals from the four satellites. 


Il. SYSTEXPMIGCwEL 


A. DYNAMIC MODEL 

An underwater vehicle will travel through the water with a desired heading and at 
an ordered speed. The combined external forces acting on the vehicle will cause it to 
deviate from the intended track at some angle (Drift) and velocity (Set). This deviation 
can be modeled as a single velocity vector when added to the vehicle’s own velocity 
vector results in the True Velocity vector along which the velucle wall travel) Ditgaaes 
lationship is shown in Figure 3 on page 7, where « is the vehicle’s commanded heading, 
y is the angle for the True Velocity. and w 1s the angle of the drifts velocity. [ie /Simamie 
addition of the indvidual components produce Equations 2.1 and 2.2 for the x and y 


components of Vt, x; and y;. 
xr = Velcos(a) + Set cos(y) (Zale 
Vr= Vel sin(a) + Set sin(w) (2:2) 


Developed in Reference 2, the vehicle’s dynamic model. neglecting any sway dv- 
namics, 1s illustrated in Figure 4+ on page 8. In this model. a heading input (Hdg) 
produces a heading direction ( « ), whose sine aud cosine are multiplied bv the vehicle's 
ordered speed (Vel) to produce the ordered velocity components (x andy). If these 
components are summed with their respective drift components (zx and wy), then the 
components of the true velocity (.c, and y,) are obtained. 

G(s) is the transfer function deseribing the vehicle’s true dynamics, which for the 
generic veliucle used in this thesis will be a simplified first order svstem. With & = 4.4 arid 
: l 
ye st+4 
IT~DGCOM 1s the apphed heading command and ALPHA is the resultant vehicle head- 





, a transient response as shown in Figure 5 on page 9 is realized 


ing angle ( a@ ). This G(s) was chosen as a reduced order model based on various vehicles 
Which are now in operation. K=4.4 produces a 2 sec. settling ume with little overshoot. 
This type of response will minimize the effect of the vehicle’s response to heading com- 
mands on the Kalman Estimator updating during the simulations in Chapter 4 and 1s 
close to what a real vehicle would produce. 

As can be seen from the model, this is an open loop system where the heading angle 


is put in from an outside source and maintained by the position feedback of a. Thus 
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Figure 3. Velocity Components Encountered by the Vehicle 
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Figure 4+. Dynamic Model [ Adapted from Ref. 2 
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Figure 5. Step Response for Vehicle 





heading command will be provided by the processor after calculating ux and wy and de- 


ternuning an « which will negate these inputs. 


B. STATE SPACE REPRESENTATION 

In this section a discrete state space model is developed based on the dynamic model 
above along with the identification of the individual states. 

The linear discrete state space model is described by Equations 2.3 and 2.4 [Ref.3: 
pp. 66-67] where x, is the current value of the states at time =?¢ and x,., contains the 
predicted values of the states at ttme=r+ 7. T is defined as the sampling period. The 


definitions for the variables in the equations below are summarized in Figure 6. 


Keay = Oxy + Vy + Powe (2.3) 


a ORGS (2.4) 


a 


x= ~< sige vector 

u, = fx lvector of applied forcing functions 
w, = Kx 1 vector of random forcing functions 
z=L~x 1 observation vector 

v= Lx] meastirement noise vector 

(P= NV x N Transition matrix 

[, = x ef matrix 


> =Nx K matrix 


H=LxN matrix 





Figure 6. Symbol Identification 


The following states were defined and placed into x: 


0s (2.5) 
x(2) =x, (2.56) 
(3) = y, (2.56) 
x(4) = y, (2.94) 


x(5) = ux (2.5e) 
Oa C2579) 


tie Oe watrix becomes 


iaeno OF 0 
0) TOO OC 
DO Fro 
= (2.6) 
pmo 100 
OmvrO 0 10 
000001 


The command vector, u,, will consist of the velocity components, ix, and uy,, of anv 
new heading given to the vehicle which are needed to compensate for the estimated set 
and drift. Thev are given in the equations below with Hdg, and Hdg,_, as the heading 


commands at time= KT and (k-1)T respectively. 


ux, = Vel x ( cos(Hdg,) — cos( Hdg,_,)) (7) 


uy, = Vel x ( sin(/7dg,) — sin(//dg,_,)) (275) 


i, now becomes: 


Oo coc CO KF HY 
> ee oS 


iiesx and y components of the Set and Drift are contained in w, as wx and wy. 
Since the Set’s magnitude and the Drift’s direction are assumed to be white Gaussian 
random variables with zero mean about predicted values, wx and wy will also be white 
Gaussian random variables with zero mean about their predicted values. This leads to 
i oclow. 


l1 


(2.10) 


SCF COCO F WY 


Q 
0 
T 
l 
0 
l 


At this point, the observation vector, z, consists only of the position coordinates 
(x and y) which come from GPS. The velocity components received froni GPS are those 
coniponents that the vehicle is experiencing at that instant in time and may or mav not 
be the time averaged velocities which are defined in the model. If onlv the position co- 
ordinates are used then the system is not observable [Ref. 3: pr 67-70]. As a result an- 
other variable must be measured. 

In the system model ( Figure 4 on page 8 ), recall that « 1s fedback to the input in 
order to maintain the applied heading command. Since this value must be measured, it 
can serve as the other variable that will link the states. Thus the observation vector 


becomes: 
z=|y,|+y (2es 


It can be observed that z is now nonlinear because x 1s not a linear function of the 
states x(2), x(4), x(5). and .x(6). [Eq. 2.12] As a result, it cannot be treated as tieiiie ms 
function as described in Equation 2.4. 


e= arctan] (25 


x(4) — x(6) 


x(2) — x(5) 


The next step is to create an estimator that will measure wy and im 1 The pieseame 
of noise (uncertainty) in the measurements and overcome nonlinear problem of the ob- 


servation vector. In Chapter 3, this is accomplished by the design of an extended 


Kalman Filter. 


WI. STATE ESTIMATION 


A. INTRODUCTION 

In this chapter, the theory of statistical parameter estimation 1s applied to deter- 
nuning the values of the Set and Drift components, wx and uy, which were discussed in 
Chapter 2. Because of the nonlinear measurement vector and the uncertainty generated 
impede measurement noise, a simple observer, 1.e., the Luenberger Observer, is not feasi- 
bles wwe to the successful applications of the Extended Kalman Filter [Ref. 4: p. 23] in 
solving nonlinear estimation problems and the advancements in microprocessor design 
in obtaining real time solutions, the Extended Kalman Filter was chosen as the estimator 


for this problem. 


B. KALMAN FILTER 
1. he Linear Kalman Filter 

The Kalman Filter is an algorithm for extracting or estimating information from 
noisy data. lore specifically, it 1s a parametric estimation process based on an 
autoregresive (AR) model of the plant. The filter uses samples or measurements of the 
plant as observables to recursively update estimates of the plant’s state. 

When both the system and measurement models are linear functions of the 
States. The state update equation is shown below in Equations 3.1 and 3.2. In these 
equations G 1s the vector containing the optimum estimation gains (Kalman gains), 
X,,-1 is the predicted value of the states at time =k T based on the measurements ob- 
tained and anv known forcing functions at time =(k — 1)T. and x, is the updated state 


estimation vector based on the new measurement vector (z,). 
A A A os 
Lik = Leip) + Glzy — AXy g-)) (3.1) 


The following assumptions are made in using the Kalman equations: 


e The random forcing function (1,) is zero mean and uncorrelated with covariance 
oF. 


Elw,] =0, forall k>0 


eno ec onal k=} 
EL wz; ] _ fi for all kA #/j 


e The measurement noise (4,) is Zero mean and uncorrelated with covariance R,. 


Ev) = On orale = 


| (3.3) 
E[v vs] = Rep kp for all k =j| 
ae O for all k 4; | 
e The random forcing function and measurement noise are uncorrellated. 
ECsy,v/ J] = ELy,ws ] = 0 for all ky #0 (3. 
e The random forcing function and initial states are uncorrellated. 
ELw,xd ] = ELxywe] =0 for all k #0 (3.5) 
e The measurement noise and initial states are uncorrellated. 
ELr xe J = Elxri ] = 0 for all A #0 (3.6) 


The optimal estimation gains are those which satisfv the following relationships: 


kp = Pry pares Hy Pepa ome RY" (3.7) 

Re Cl = Cea ee (3.8) 
is aE . 

Pray = PPy yD +P,Q0'T (3.9) 


where P,., is the error covariance matrix update at time =x 7 and P,.,, is the prediemes 
error covariance at time = (k + 1)T based on the data at time = 4 T. 
2. Extended Kalman Filter 
As previously mentioned, the measurement vector as described in Equations 
2.11 and 2.12 is a nonlinear functon of the states and as a result will be modeleqman 


shown below, 
zy = h(yk) + 2 (3.10) 


In this model, the measurement vector, z,, 1s a funetion of the state varaeles 
plus the measurement noise vector, V, The adaptation of the Kalman Filter involves the 
linearizing of A(x,.k) by taking the Tavlor Series expansion about the predieted value of 
the states, x,,_,, at time =A F and keeping only the first order terms, "Tits sete 
tended Kalman Filter. The reader is referred to Gelb [Ref. 3: pp 180-225) for aden 
explanation of the process. In summary, a more precise approximation can be achieved 
in the optimal estimator by ineluding higher terms of the series expansion but at the 
expense of simplicty and increasing the possibility of the recursion diverging. 


The linearized form of Equation 3 /0melds 





Zy = Hy Xp t+ Ye (3.11) 


Chix,.&) 
= a . Cele) 
CX, Xp = Xee-] 
A summary of the definitions for the Extended Kalman Filter estimator for the 
linear state model [Eq. 2.3] and the linearized measurement model (Eq. 3.11] is displaved 


in Figure 7 on page 16. A block diagram that illustrates the algorithm for recursively 


computing the filter is shown in Figure 8 on page 17. 


C. MATRIX CALCULATIONS 
{. Determination of O 
O’ as defined in Equation 3.2 1s the covariance matrix for the random forcing 
function of the set and drift velocity components. It is a measure of the uncertainty 
contained in the predictions. As stated previously, the set and drift experienced by the 
vehicle is assumed to be Gaussian with 0 mean about the predicted values and as a result 
their x,y components are independent and will be approximately Gaussian with 0 mean 


with the standard deviations equal to the maximum expected deviation in the x and v¥ 


mirections. [hus Q’ 


Ox = ee 4 (3-16) 


wx 


The method used as an approximation to calculate o?, and o?, is the following: 


¢ Determine the maximum amount of variation which can be expected in the set and 
drift. 


¢ Calculate the cartesian components, x and y, from the values determined above. 
Ser these values equal to 3o,,, 


e Divide the x and ¥ components by 3, subtract them from their respective predicted 
components, and square to obtain o2, and o?,. 


Wx 


AS an example, let the maximum expected deviation for the drift be + 20° and 
the maximum expected deviation for the set be + .5 knots. The predicted values for the 
Semeeond Drift are 45° and1.0 knots. Using the above procedure, 


o(, — 0.45 and @?, = 0.43. 


SYSTEM MODEL: x,,, = Ox, + Ty, + Pas, 
where w, oc .VL0,Q’ J 


MEASURE MODEL: z, = h(xp4) + 
where ¥, cc .VL0,Q’ J 


[INITIAL CONDITIONS yee eens 
OTHER ASSUMPTIONS: ELu,»/ ] =0 for all k= 0. 


GAIN EOUAgIO; 
a Tr 
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ERROR COVARIANCE UPDATE: 
Prk = LI- G, fT, 1P, k—| 


STATE ESTIMATION UPDATE EG ey 


A A = A 
Nick = Se pay 7 GL, = A(Xpy1)J 


ERROR COVARIANCE PROPAGATION ECUA es 
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ee ie ae 


STATE ESTIMATION PROPAG Oe. 


AN A 
Xepye = Pe FU iy 


An(x,,k) 


a A 
OXp Xe = Xkik-l 


DEFINITIONS: I, = 
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Summary of Extended Kalman Equations 
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Fieure 8. Kalman Tilter Recursive Loop 
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2. Determination of R 
Ras defined in Equation 3.3, 1s the covariance matrix for the treasurenmess 
noise Vector. Asin Q’, Ris the uncertainty contained in the measuring of the three pa- 
rameters x, v, and a in the presence of noise. For this application, the noise for all three 
parameters 1s assumed to be uncorrellated amongst themselves and white Gaussian with 


Onmedan- sik oecomes 


ot 0 0 
ig aw, . 0 (3.17) 
D0 ey, 


The variances, a, and o,, are the standard :.ations from GPS and are obtained 
by using the stated DOD value for the Circular Error of Probability (CEP) of 10 omieiers 
or whichever CEP figure is associated with the GPS Receiver in use and Equationegmms 
(Ref. 5: p. 21]. The standard deviation for the specific type of compass used is repres- 


ented oy 0, 
Che) (4.2.4) (3. 13} 


3. Determination of x,,,, 
Xp,-, as defined in Equation 3.1 is a predictor of the states at time = & + | based 
on the current estimation of the states at time=& As a result, x,.,, has to take into 
account any controls which are applied as a new heading command to overcoine the 


estimated set and drift as well as the known plant dynamics. Thus x,.,, becomes 
A A - 
Xepiy = Oxy + Tuy (3.19) 


wihere [,,.<D, and #7, were-delined tmianiawone.rs, 
4. Determination of 4, 
ff, as defined in Equation 3.12 is the linearized observation vcctor anduiseees 
tained by taking the partial derivative of z with respect tom. Recalling Equationeeaies 
the first two values of z, x and v, are hnear functions of only one state and their partial 


derivatives are easily computed in Equations 3.20 and 3.21. 


vy 











z=|/¥ 
, cran le) ly) 
cr ear 
x2) — x) 
=e) OOO 
CX Cx 
Spal 2! ny 
2 FO 0g000) 
Ox Cx 


To obtain the partial derivative 


meet: pp 328-350} 











x(2) — x(5) 


d arctan(u) = l du 
dx 7 Pees ye es 
a ei 
dx ye als 
Soiielaumations 3.21 and 3.22 
e7AS 
achieved for Ag 
Cx 
Garctan ote SO} 
a) 2) = x3) 
6x(1)  éx(1) 
Parcran ole) Rel 
— LOY, ae 
Cala)» Me MD) 
ood) 7 Cx(2) 
Carctan passe wie) 
Se ae ee 
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of z(3), the following relationships are used. 


and siumplifving, the following results were 
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x(+) — x(6) 


6x(3) Oarctan M2)— x5) _ So (3.28) 

Bx(5) — 6x(5) (x(2) — x(5))° + (xe(4) — x(6))? ‘ 
Carctan esGie iy) 

6z(3) _ 12) 020) OE re 

éx(6)  a.x(6) (x(2) — x(5))* + (x(4) — x(0))? 


The above equations are combined into Equation 3.30. which is the matrix ver- 


sion for /7,. The final version. it should be noted, contains only three major calculations: 


the denominator term which is common, the x{2) — x{5) term, and the x(4)— x(o)atgm 


The two terms, H;, and ,, are just the negatives of H,, and H,, respectively. [iusmaae 


linearization is not as ominous as one would expect and can easily be implemented on 


A MuCcrOprOcessor 


where 


1 0 60 Ce ae 
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IV. SIMULATION RESULTS 


A. INTRODUCTION 

mets Chapter, the Extended Kalmen Filter calculated in Chapter 3 is tested by 
performing various simulations using the models developed in Chapter 2 and determin- 
ing if the filter is correctly estimating the six states. This testing was done in four steps. 
In the first step, the filter and program were checked to insure that the filter accurately 
estimated the states and that the program interacted with the filter while maintaining the 
vehicle’s track with no added bias. Next, a bias was added to the predicted values of Set 
and Drift and a heading change is made to correct for the bias and allow the vehicle to 
continue to its destination. This test would show whether the filter would accept the 
heading change and maintain its lock. Third, the filter’s abilitv to handle multiple Set 
and Drift changes is checked by randomly changing the bias every other sample. Last, 
random noise is added to the measurements being fed into the filter and the errors 
measured. 

The full simulation program which was used to run the last simulation is shown in 
Appendix A. The other simulations were run with thus program with the unnecessary 
segments commented out. The program primarily consists of Input, Initialization, Dy- 
namic, State Estimation, and Correction Segments. In the Input, the values for the ve- 
hicle’s speed, the origin and destination coordinates, the predicted values for the Set and 
Pit, and the*standard deviations for the various random variables are entered. The 
units used for the distance and angle inputs are nautical miles and degrees. 

The Initialization segment the nautical miles, hours, and degree units are conveted 
to feet, seconds, and radian units of measure. In addition, equidistant Waypoints and 
course and distance to destination are calcluated, biases are added to the predicted Set 
and Drift, and the Kalman Filter matricies are initialized. 

The Dynamic segment calculates the actual x and yv coordinates and the current 
heading angle which are used as the GPS and compass inputs into the filter by using the 
model of Figure 4 on page 8 created in Chapter 2. 

ie State Estimation wpdates //,. h( Xu), and z and then calls the subroutine 
Kalman [Appendix BJ]. Kalman performs the matrix operations and returns the current 
state estimate, X,,, the projected covariance of error, P,.,,, and the projected state esti- 


ieee, ,,. Finallv the returned estimates of wx and wy are used to calculate the new 


21 


heading command in the Course Correction segment which, in addition, updates x,.,, 
Git. oat}: 

For alt the simulations, the velicle’s ordered speed or speed through the water is 
maintained as a constant. This assumes that the vehicle has an optimum speed which 
muninuzes the amount of fuel consumed during a long distance voyage, which appears 
to be the trend in current research. The program could easily be modified for the vemmer. 
tO Maintain a constant over the ground speed. The speed picked for all the runs was 75 
knots. 

The predicted Set and Drift were held as a constant for all the runs at 45 degrees and 
1.0 knots. The o,, and a,, values were those calculated in the example on Page 15 as .66. 
lor o, and a,, the values used were determined using Equation 3.18 and the DOD figure 
of 10 meters for the CEP. Thus, o, = o, = 25.4 feet. The value for a,, choser at random, 
was 1.5 degrees or .03 radians. A summary of the constants and their values are sum- 


Moarizedun lobles 


Table 1. SUMMARY OF VALUES USED IN SIMULATION RUNS 


CONSTANT 
dt Seconds 
sts 
0.0.0.0 Nautical Miles 


Destination Coordinates 7.0.6.0 Nautical Miles 
25.4 feet 





.O3 radians 
06 feet per secana 


.66 feet per second 


For each simulation run, there are 7 graphs as output. The first one shows the ve- 
hicle’s overall track as compared to the desired track using the waypoints as a reference. 
The remainder are plots of the current state estimates shown against the actual states 
as a function of time. The values for the actual states are graphed as solid lines. while 


the estimated state graphs use a dashed line. For convenience to the reader, the actual 


number of figures are reduced by combining two plots into each figure with the excep- 


tion of the velicle’s overall position track. 


B. STATE ESTIMATION WITH NO BIAS 

In this simulation, the predicted Set and Drift are entered into the vehicle and 1t 1s 
instructed to maintain its course to the destination point. There 1s no added bias to the 
Set and Drift and the run 1s performed for 6000 seconds or 5 sampling periods. 

Figure 9 on page 24 shows the vehicle’s actual track against the intended track. 
The waypoints are shown as a reference. As can be seen, the vehicle follows the intended 
track with no deviation which demonstrates that the program itself is functioning prop- 
erly. Figure 10 on page 25 provides the estimated and actual position state variables 
plotted both as a function of time and each other. In Figure 11 on page 26, the true 
velocity state variables, x and y, are again plotted as functions of time and estimated vs. 
actual. Figure 12 on page 27 plots the estimated and actual Set and Drift components, 
ux and wy in the same manner as before. These figures demonstrate that the filter is 


locking on after onlv one sample and maintains the lock throughout the run as expected. 
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Figure 11. Velocity Components With No Added Bias 
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C. STATE ESTIMATION WITH BIAS AND COURSE CG Rreciia 

During this simulation, the filter’s abilitv to maintain track during a course change 
was determined. A bias of .2 radians was added to the drift and .5 knots was added to 
the Set. After the filter determined an estimate for the Set and Drift compenenismams 
simulation program added them to a new desired course to arrive at the new heading 
command and updated x,_,,. The new desired course was calculated by comparing the 
vehicle's current position to it’s destination and subtracting the respective components. 
Aswas previously done, the length of the run was 6000 seconds. The results are dis- 
plaved in Figure 13 on page 29. Figure 14 on page 30, Figure 15 on page aie 
Pare lovon page 32. 

The figures demonstrate the program and filter’s abilitv to correctly determine the 
overall Set and Drift and give a correct heading command to guide the vehicle to its 
destination. As can be seen, the filter successfully acquires wx and uy after only one 
samiple and miaintains a lock there after. This allows the vehicle to stay on course 
without having to constantly make corrections due to bad estimates from the filter: 


Next, the filter’s ability to handle multiple Set and Drift changes will be tested. 
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Figure 13. Intended and Actual Vehicle Tracks With Bias and Course Correction 
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Figure 14. = Position Coordinates With Bias and Course Correction 
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Figure 15. Velocity Components With Bias and Course Correction 
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D. STATE ESTIMATION WITH MULTIPLE SET AND DRIFT CHANGES 

In this simulation, the Set and Drift was varied in a random manner at the beginning 
of every other sample. Values were created using the Normal function in DSL and 
added to the predicted Set and Drift. The Normal function randomly generates num- 
bers that are distributed according to a Gaussian distribution function about a mean 
with a standard deviation. In this case the mean was zero and the standard deviation 
for the Drift was 7 degrees or .12 radians and .2 knots for the Set. The run was for 4200 
seconds in order to obtain multiple variations at the sampling period of 600 seconds. 
The results were plotted and are displaved in the same manner as the previous two sim- 
ulations. 

In Figure 17 on page 34, the vehicle’s actual track is off the intended track bv as 
much as 500 feet which could be critical in any actual mission performed by a vehicle 
equipped with this system as its onlv navigation source. The rest of the graphs as shown 
in Figure 18 on page 35, Figure 19 on page 36, and Figure 20 on page 37 show the fil- 
ter s ability to track any deviations in the Set and Drift experienced by the vehicle with 


only one sample after the change. 
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Figure 17. Intended and Actual Vehicle Tracks With Multiple Set and Drift 
Changes 
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E. STATE ESTIMATION WITH MEASUREMENT Nei 

In this simulation noise was added to the measurements of x, y, and w in order to 
determine how the Kalman Filter would respond. This noise was added using the 
Normal function described in the previous section. A different function was generated 
for each measurement with unique seed numbers, in order to preserve their independ- 
ence, and zero mean about the actual value. The resulting graphs are displaved in 
Figure 21 on page 40, Figure 22 on page 41, Figure 23 on page 42, and Figure 24 on 
page 43. The run time was 4200 seconds and as in the previous simulation the Set and 
Drift were changed every other sample. 

The graphs show the same results which were achieved in the third simulation with 
some smalt differences. The vehicle's mtended and actual tracks are much closeruiaam 
before which is due to a lesser amount of deviation in the Set and Drift variables. A 
close look at the plots of the state variables show some Variation between their estimated 
and actual values which was not present before. These are the errors produced by the 
measurement noise. In order to see the actual differences, three tables were Seneqamem 
which show the actual and estimated values against time. The position coordinates, X 
and Y, are shown in Table 2 on page 39, x andy are in lable 3 on page 97igage 
ux and uy are in Table 4 on page 39. 

From the tables, the errors range from 3 feet up to 2! feet for x and y, are lessutiiam 
.O2 feet per second for x andy, and vary up to .[S feet per second for ux and dye 
also that during the periods when there is no change in the Set and Drift, the estimates 
Show a significant improvement. Errors such as these may be tolerable depending on the 
vehicle's assigned mission. The best way to nuninuze the error would be to take multiple 
GPS readings in order to obtain a better fix. But this would increase the amount of time 


thatthe vemrele would seendson thesmriace 
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Table 2. DIFFERENCES IN POSITION COORDINATES 


TIME 


ACTUAL ESTIMATED ACTUAL ESTIMATED 
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Table 3. DIFFERENCES IN VELOCITY COMPONENTS 
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Table 4. DIFFERENCES IN SET AND DRIFT COMPONENTS 
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Vv. CONCUDUSTIOR 


A. ANALYSIS 

The simulation results from Chapter 4 demonstrated the Extended Kalman Filter’s 
abilitv to successfully identifv the total Set and Drift that an underwater vehicle could 
experience On its journey. However, there are some limitations which have appeared 
First, if the magnitude of the Set and Drift vary extensively between samples) tiemmame 
velucle’s ability to navigate precise tracks is severely limited. There are three mayan 
factors which affect how far the vehicle will be driven off its intended course; the mag- 
nitude of the Set and Drift, the vehicle's ordered speed, and the time between Sammguess 
There is no control over the first factor but there ts some latitude over which thevogen 
ator can control. Increasing the speed and decreasing the sampling interval will allow 
less deviation from an intended track. However, both the resulting tncrease in power 
consumption and the increase in the probabilitv of detection will have to be taken into 
aAceOmnC 

Another major limitation with using thus system as a vehicle’s sole navigation source 
is that the covert aspect of an underwater vehicle becomes lost awhen the GPS fixes are 
taken. Because of the frequencies at which GPS operates have very little ability to travel 
through water, the vehicle’s antenna must break through the water’s surface. To muint- 
mize the risk of detection, a system would have to be designed which would allow the 
antenna to be raised and lowered from a safe depth. An alternative would be to use the 
Omega Navigation System, whose frequencies can be reach down to about 40 feet, as a 
prime source and use GPS as an update at regular intervals or whenever the Omega data 
is suspect. This technique could allow the vehicle to refrain from surfacing as much and 


at a rate comparable to that required in updating an [MIU system. 


Bo CAPPERICAITONS 
I. Back up to an IMU 
The most obvious place for this design 1s as a backup to an [MIL system 
onboard an underwater vehicle which would take over in event of a failure. Current 
design philosophy seems to be centered on using conventional dead reckoning systems 
in Which the angles and components are calculated using trigonometric functions. These 
type of complex calculations are difficult for a microprocessor to accomplish and ma- 


chine roundoff errors can occur if the angles are close together. This Extended Kalman 


ot 


Filter design requires only one trigonometric function [Eqt. 3.15] and the matrix 
calcualations are ideally suited for a microprocessor. 
2. Mine Location 

Because this system 1s lightweight, inexpensive, and has verv little material 
which will disturb a magnetic field, a large fleet of plastic mine locating escorts could 
be developed. These escorts could lead ships through a mined area by locating suspected 
mine positions. As such, the vehicle would be composed of a body, a propulsion unit, 
a sunple attitude control section, and a detector. This detector would be the most ex- 
pensive component and it could take the form ofa high resolution sonar or a magnetic 
anomialy detector. The shape of the vehicle could be designed that would reduce any 
pressure Waves which would set off anv pressure sensitive mines. The antenna system 
would not have to be as complicated as mentioned previously because the «eed to op- 
erate covertly would be erased. The communication link could be fiber optic and go to 
a mother ship or a group of mother ships working together. 

3. Long Range Reconnaisance 

A variant of the mine locating vehicle discussed above, would be as a long range 
reconnaisance vehicle with the mine detector replaced with some other sensor such as a 
conimunications receiver or an infrared detector. The nussions would be limited to those 
which do not require precise navigation such as coast or ship surveillance. The advan- 
tages to using this system is that the power needed to run an [MU would be eliminated 
and and the costs significantly reduced. Communication could be in the form of fiber 
optics for realtime information. Radio or data storage systems could be used for non- 
realtime. Delivery and retrieval could be accomplished through the use of a submarine, 


a ship, or small boats. 


C. RECOMMENDATIONS FOR FUTURE RESEARCH 
The following areas present valid opportunities for useful expansion of this work: 


e Develop an antenna and support svstem, in order to receive the GPS signals in the 
presence of low to medium sea state and allow the vehicle to remain at depth. 


®* Build a prototype Extended Kalman Filter and test it as a back up on an existing 
autonomous underwater vehicle. 


APPENDIX A. SIMULATION PROGRAM 


This program simulates an underwater vehicle traveling through the water and 1s 
written in DSL. The Dynamic section mimics the vehicle's true dynamics; Whicha aa. 
modeled in Chapter 2 as a second order svstem, and thereby calculates the vehicles true 
position and heading angle. The Sample section samples the system’s position and 
heading angle every T seconds and these measurements are sent to the subroutine 
Kalman which estimates the states. After the estimates are returned, the program cal- 


culates a new heading command and updates the predicted state vector XP. 


TITLE VEHICLE SIM WITH KALMAN CORRECTION TO DEST / VAR F AND BIAS 
PARAM VEL=7.5,FVEL=1., ,PANG=45., N=10 Ni=7enNg—lom nee 
PARAM AN=0) (AY=0.9) Sev=92 sere 
PARAM SIGCA=0.3,SIGCV=0.3 ,SIGX2=100. ,SIGY2=100. |)SevAy2—se 
SIGCY=.1 ,SIGEX= 1 /SIGX=10, (sie@1—10 me 
D DIMENSION WAPNTX(0: 10) ,WAPNTY(0: 10) 
D DIMENSION F(6,6),XHAT(6),XP(6),Y(3),Q(2,2),R(3,3),P(6,6),H(3), 
D 1 HP(3,6),G(6,2),XM(6) 
EXCLUDE IER,F,XHAT,XP,Y,Q,R,P,H,HP,G, XM 
D DATA F,XHAT,HP,Y,Q,R,P,H,XP,G / 133 *0. / 
FIXED 1,J,N,N1,N2,N3 
INITIAL 
= 2 
ie 
A=0) 
TIMEW=O. 
T=DELS 
** ADDITION OF ERROR TO PREDICTED CURRENT VELOCITY AND ANGLE 
FANGS=( FANG+180. )*DEGRA + . 2 
FVELS=FVEL 


FVELS=( FVELS*6000. )/3600. 
FVEL=( FVEL**6000. )/3600. 
VEL=( VEL**6000. )/3600. 
BX=BX"6000. 
BY=BY*6000 
UXDOTS=FVELS*COS( FANGS) 
UYDOTS=FVELS*SIN( FANGS) 
DC=ATAN2( BY-AY , BX-AX) 
DIST=SQRT( ( BX-AX)**2+( BY-AY) #2) 
INT=DIST/10. 
DO 10 I=0,10 
WAPNTX(I)=I * INT*COS(DC) 
WAPNTY(I)=I *%* INT*SIN(DC) 
WX=WAPNTX( I) 
WY=WAPNTY(L) 
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CALL SAVE(S3) 
CONTINUE 


INITIALIZE KALMAN MATRICIES 


ENG At Aer Ht 


DO 11 I=1,6 
F(I,1I)=1. 
CONTINUE 
F(1,2)=T 
F(3,4)=T 


INITIALIZE Q 


Q(1,1)=SIGCx 
O(2,.2)=s16Ey 


INITIALIZE GAMMA1 


ai, hae 
C= 
G(3,2)=T 
G(4,2)=1. 
eS. Dae 
Cccn = 


INITIALIZE R 


Rei )=ste 2 
R626? =se 
R(3,3)=SGYAW2 


INITIALIZE XHAT WITH PREDICTED VALUESOF CURRENTCOMPONENTS 


XHAT(5)=0. 
XHAT(6)=0. 

XHAT(5)=FVEL * COS((FANG+180. )**DEGRA) 
XHAT(6)=FVEL * SIN((FANG+180. )**DEGRA) 


INITIALIZE HP 


HeCl. 1j=1. 
neGZ..3)= 1. 


DETERMINATION OF ACTUAL X AND Y COORDINATES(SUNS INPUT) 


DERIVATIVE 


HDGERR=YAW - HDG 
=-4.41*HDGERR 
YAWDOT=REALPL(O. ,. 25,K) 
YAW=INTGRL(CO. , YAWDOT) 
XDOT=VEL*COS( YAW) 
SDOTS=XDOT+UXDOTS 
YDOT=SIN(C YAW) *VEL 
YDOTS=UYDOTS+YDOT 
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Y1=INTGRUCO Ons. 
X=INTGRL(0. ,XDOTS) 
SAMPLE 
LF(TIME. EQ. 0. )THEN 
FANG=FANG ** DEGRA 
IF(SIN(FANG)*SIN(DC). GT. 0)THEN 
ALPHA=PI-ABS( FANG )+DC 
BETA=AS IN( FVEL*S IN( ALPHA) /VEL) 
HDG=DC + BETA 
ELSE 
ALPHA=PI -ABS( FANG) -DC 
BETA=ASIN( FVEL*SIN( ALPHA) /VEL) 
HDG=DC - BETA 
ENDIF 
Xs HDG=DC 
XHAT( 2)=VEL*COS(HDG) 
XHAT( 4)=VEL**SIN( HDG) 
XP( 2)=VEL*COS(HDG) 
XP(4)=VEL**SIN( HDG) 
YC. PeaDe 
DO 25 I=1,6 
KM(I)=XP(I) 
25 CONTINUE 
CALL KALMAN(A,XHAT,F,XP,Y,Q,R,P,H,HP,G, IER) 
ELSE 
A=A + 1. 
** DETERMINATION OF REQUIRED COURSE TO DESTINATION 
30 DWY=WAPNTY(10)- Y1 
DWX=WAPNTK(10)- X 
DC=ATAN2(DWY , DWX) 
“e CALCULATE FORCE COMPONENTS,UANG AND UVEL 


* UPDATE H, 4, AND HP MATRICIES 


H(1)=XP(1) 
H(2)=XP(3) 
H(3)=ATAN2(XP(4)-X?(6) ,XP(2)-XP(5)) 
DEN=( XP(2)-XP(5))**2 + (XP(4)-XP(6))**2 
HP(3,2)=(XP(6)-XP(4))/DEN 
HP(3,4)=(XP(2)-XP(5))/DEN 
HP(3,5)=-HP(3,2) 
HP(3,6)=-HP(3,4) 
Y(1)=X + NORMAL(N1,0,SIGX) 
Y(2)=Y1 + NORMAL(N2,0,SIGY) 
Y(3)=YAW+ NORMAL(N3,0,SIGYAW) 
DO 35 I=1,6 
XM(I)=KP(I) 
35 CONTINUE 


as KALMAN FILTER UPDATE OF XHAT 
CALL KALMANCA,XHAT,F,XP,Y,0,R,P,H, He yGatery 
CALCULATION OF ESTIMATED CURRENT VELOCITY AND ANGLE FROM UPDATED XHAT 


UVEL=SQRT( XHAT( 5 )****2+XHAT(6)**2) 


oy 


UANG=ATAN( XHAT( 6) /XHAT(5) ) 
CALCULATION OF STEERED COURSES TO WAYPOINT AND DESTINATION * 


IF(A. EQ. Al-1. ) THEN 

IF( SINC UANG)**SIN( HDG). GT. 0) THEN 
ALPHA=P1-ABS(UANG)+DC 
BETA=ASIN( UVEL*SIN( ALPHA) /VEL) 
HDGD=DC + BETA 

ELSE 
ALPHA=PI-ABS(UANG)-DC 
BETA=AS IN( UVEL*S IN( ALPHA) /VEL) 
HDGD=DC - BETA 

ENDIF 


UPDATE X(K+1/K) FOR CRSE CHANGE 


XP(2)= VEL * COS(HDGD) + XHAT(5) 
XP(1)= T * XP(2) + XHAT(1) 
XP(4)= VEL * SIN(HDGD)+ XHAT(6) 
XP(3)= T * XP(4) + XHAT(3) 
HDG=HDGD 
ELSE 
HDG=HDG 
ENDIF 

ENDIF 


DYNAMIC 
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IF(A. EQ. Al) THEN 
BIAS=NORMAL(N,0. ,. 3) 
UXDOTS=FVELS:*COS( FANGS+BIAS) 
UYDOTS=FVELS*SIN( FANGS+BIAS ) 
A1=A1+2. 

TI=TIME + 2. 

ELSE 
UXDOTS=UXDOTS 
UYDOTS=UYDOTS 

ENDIF 

PICrINESEE. Ti)CALL SAVE(S2) 

X1KK=XHAT( 1) 

X2KK=XHAT( 2) 

X3KK=KHAT(3) 

X4KK=XHAT(4) 

XSKK=XHAT(5) 

X6KK=XHAT( 6) 

X1KKM1=KM(1) 

X2KKM1=XM( 2) 

X3KKM1=XM(3) 

X4KKM1=XM(4) 

X5KKM1=KM(5) 

X6KKM1=XM( 6) 

IF(X. GE. BX) CALL ENDRUN 


CONTROL FINTIM=9000. ,DELS=600. 
METHOD ADAMS 


SAVE 


SAVE 


(S1) 600. ,X1KK,X2KK,X3KK,X4KK,X5KK,X6KK,... 
X1KKM1,X2KKM1, X3KKM1 , X4KKM1, XSKKM1, X6KKM1 
(S2) 600. ,X,Y1,UXDOTS , UYDOTS, XDOTS , YDOTS 
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SAVE 

PRINT 
GRAPH 
GRAPH 


GRAPH 
GRAPH 
GRAPH 
GRAPH 
GRAPH 
GRAPH 


GRAPH 
GRAPH 


GRAPH 
GRAPH 


GRAPH 
GRAPH 


(S3)4000. ,WX,WY 

600, HDG,X,Y1,UXDOTS.UYDOTS 

(G1/S2,DE=TEK618)X(SC=8. 4E3),Y1(SC=10. 5E3, LO=0. ) 
(G2/S3,DE=TEK618 ,OV)WX(SC=8. 4E3,AX=OMIT),... 

WY (MA=4 , SC=10. 5E3, AX=OMIT) 
(G3/S2,DE=TEK618)TIME ,X( LI=1, AX=OMIT, LO=0) 

(G4/S1, DE=TEK618 , OV)TIME , X1KK( LI=2 , LO=0) ,X1KKM1( LI=3 , LO=0) 
(G5/S2,DE=TEK618)TIME, Y1( LI=1,AX=OMIT, LO=0) 
(G6/S1,DE=TEK618 ,OV)TIME , X3KK( LI=2, LO=0) , X3KKM1( LI=3, LO=0) 
(G7/S2,DE=TEK618)TIME , XDOTS( AX=OMIT, LI=1, LO=0. ,SC=1. 8) 
(G8/S1, DE=TEK618,OV)TIME ,X2KK( LI=2, LO=0,SC=1. 8),... 
X2KKM1( LI=3,LO=0. ,SC=1. 8) 
(G9/S2,DE=TEK618)TIME , YDOTS( AX=OMIT, LI=1, LO=-1. 5, SC=1. 5) 
(G10/S1, DE=TEK6 18 , OV) TIME, X4KK( LI=2 , LO=-1.5,SC=1.5),... 
X4KKM1( LI=3 , LO=-1.5,SC=1. 5) 

(G11/S2, DE=TEK618)TIME , UXDOTS( AX=OMIT, LI=1, LO=-2. 0,SC=. 5) 
(G12/S1,DE=TEK618 ,OV) TIME, XSKK( LI=2 ,LO=-2. 0,SC=.5),... 
XSKKM1( LI=3, LO=-2. 0,SC=. 5) 
(G13/S2,DE=TEK618)TIME, UYDOTS( AX=OMIT, LI=1, LO=-2. 0,SC=. 5) 
(G14/S1,DE=TEK618 ,OV) TIME, X6KK( LI=2, LO=-2.0,SC=.5),... 
X6KKM1( LI=3, LO=-2. 0,SC=. 5) 


LABEL (ALL) KALMAN CORRECTION TO DEST /BIAS= VAR/CUR CHG=2/MSMT ERROR/ 


END 
STOP 


APPENDIX B. KALMAN FILTER PROGRAM 


This program is written in Fortran 77 and performs the necessary matrix calcu- 
lations for the extended Kalman filter developed in Chapter 3. The routine calls three 
moe P subroutmes, LEQUIF, VMILLFF, and VMULFP which accomplish the ma- 
trix multiplication operations. In order for Kalman to be compatible with DSL, it was 
Written in double precision. After performing the calculations, this subroutine returns 
Mmyespredicted Covariance matrix (P), the updated state estimate vector (XHAT), and the 


Weecicted State vector (XP) to the calling program. 


C 
SUBROUTINE KALMAN (A,X, PHI,XKKM1,Z,0,R,P,HK,HKP,GAMMA, IER) 
c SPECIFICATIONS FOR ARGUMENTS 
IMPLICIT REAL*8(A-H,0-Z) 
INTEGER IER 
DIMENSION X(6),XKKM1(6) , PHI(6,6),2(3) ,Q(2,2),R(3,3), 
1 HK(3) ,HKP(3,6),T1(6,6),T2(6,6),T3(6),P(6,6), 
p GAMMA(6,2),G(6,3) 
INTEGER oN penn 
IER = 0 
N=6 
NY=3 
NX=1 
L=2 
K=A 


WRITE(6,2)((HKP(1,J) ,J=1,6) ,I=1,3) 
WRITE(6,5)(XKKM1(J), J=1,6) 
WRITE(6,6)(HK(J), J=1,3) 
WRITE(6,12)(Z(J), J=1,3) 

c CALCULATE P IF K = ZERO 
IF (A .EQ. 0) THEN 


WRITE(6,3)((PHI(1I,J) ,J=1,6),I=1,6) 
Wee o 7 )CCOC LE, J), J=1,2),1=1,2) 
WRITE(6,8)((GAMMA( I,J), J=1,2), I=1,6) 

CALL VMULFF ( GAMMA, Q, N, L, L,N,L,T2,N,IER) 
CALL VMULFP (T2, GAMMA, N, L, N,N,N, P,N,IER) 


DO 15 I=1,6 
P(I,I)=4000. 
15 CONTINUE 
WRITE(6,13)((P(I,J),J=1,6),I=1,6) 
ELSE 


Ca C2) CO) 


CONTINUE 
ENDIF 
@ CALCULATE GAIN MATRIX G(K) 
CALL VMULFP (HKP, P,NY, N, N,NY,N,T2,N, IER) 
CALL VMULFP (T2,HKP,NY, N,NY,N,NY,T1,N, IER) 
DO 35 I = 1,NY 
DO 30 J = 1,NY 
mnie) T( 1, J )er orto. 1) 
30 CONTINUE 


a1 


35 


40 


45 
30, 


70 
75 


60 


65 


10 


20 
25 


80 
9000 


9005 


NM WN re 


CONTINUE 
CALL LEQTIF(T1,N,NY,N,T2,0,T3, IER) 
IF (IER .EQ. 0) GO TO 40 
IER = 130 
GO TO 9000 
DO 50 I = 1,NY 
DO 45 J =1,N 
GCI eae) 
CONTINUE 
CONTINUE 
WRITE(6,9)((G(I,J),J=1,3),I=1,6) 
CALCULATE P(K/K) 
CALL VMULFF (G,HKP, N,NY, N,N,NY,T2,N, IER) 
CALL VMULFF (T2, P, N, N, N,N,N,T1,N,IER) 
DO 75 I =1,N 
DO 70 J = 1,N 
P(T,J) = PCI.) are) 
CONTINUE 
CONTINUE 
WRITE(6,11)((PC(I,J), J=1,6), 1£1,6) 
CALCULATE X(K/K) 
DO 60 I = 1,NY 
T3(1) = Z(1) - HK(I) 
CONTINUE 
CALL VMULFF (G,T3, N,NY,NX,N,NY,T2,N, IER) 
DO 65 I = 1,N 
(Lae MR) ed 
CONTINUE 
WRITE(6,4)(X(J), J=1,6) 
CALCULATE P(K+1/K) 
CALL VMULFF ( PHI, P, N, N, N,N,N,T1,N,IER) 
CALL VMULFP (T1, PHI, N, N, N,N,N, P,N, IER) 
CALL VMULFF ( GAMMA, Q, N, L, L,N,L,T2,N, IER) 
CALL VMULFP (T2, GAMMA, N, L, N,N,N,T1,N, IER) 
DO 25 I =1,N 
DO 20 J = 1,N 


P(I,J) = P(I,J) + T1(I,J) 
P(I,J) = P(I,J) 
CONTINUE 
CONTINUE 


WRITE(6,1)((P(I,J), J=1,6), I=1,6) 
CALCULATE X(K+1/K)=XKKM1 
CALL VMULFF ( PHI, X, N, N,NX,N,N,T1,N,IER) 
DO eG. alee 
XKKM1(I) = Ti(I,1) 


CONTINUE 

GO TO 9005 

CONTINUE 

CALL UERTST (IER, 6HFTKALM) 

RETURN 

FORMAT(/,' PKPLK='5/,6(C3x Fl2.4 eo 
FORMAT(/,' Z2P=' «/ 3603, Fils on 
FORMAT(C/,  PHI= ./ 76(3x oi oD 
FORMAT(/,° | XKK= , /,60358 ,P 12143. 
FORMATC/;' XKKMI= </,6(6s- Fl2saeccem 
FORMAT(/,' HK=' ,/,3(3X,F12.4,3X)) 


ta 
Rw 


a 
WHF WO ®O ~I 


FORMAT( /, 
FORMAT( /, 
FORMAT( /, 
FORMAT( /, 
FORMAT( /, 
FORMAT(/, 
END 


' 
! 
! 
1 
1 
' 


Oe eye bh 1224, 3X)) 

GAMMA= 42 3b 24 3X )) 

KALMAN GAINS =' ,/,3(3X,F12. 4,3X)) 
PKR= yeo( 3h? 4 3X)) 

ey Corer 12,453) ) 

P/K=0 =' ,/,6(3X,F12. 4, 3X)) 
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